import { Vector3 } from './V3';
import {Quaternion,Orientation3D} from "./Quat"
export function lookAt(eye, at, up) {
    let data = new Float32Array(16);
    let zAxis = at.subtract(eye, Vector3.HELP_0);
    if (zAxis.length < 0.0001) {
        zAxis.z = 1;
    }
    zAxis.normalize();
    let xAxis = up.crossProduct(zAxis, Vector3.HELP_1);

    if (xAxis.length < 0.0001) {
        if (Math.abs(up.z) > 0.9999) {
            zAxis.x += 0.0001;
        } else {
            zAxis.z += 0.0001;
        }
        zAxis.normalize();
    }

    xAxis = up.crossProduct(zAxis, xAxis).normalize();

    let yAxis = zAxis.crossProduct(xAxis, Vector3.HELP_2).normalize();

    data[0] = xAxis.x;
    data[1] = yAxis.x;
    data[2] = zAxis.x;
    data[3] = 0;

    data[4] = xAxis.y;
    data[5] = yAxis.y;
    data[6] = zAxis.y;
    data[7] = 0;

    data[8] = xAxis.z;
    data[9] = yAxis.z;
    data[10] = zAxis.z;
    data[11] = 0;

    data[12] = -xAxis.dotProduct(eye);
    data[13] = -yAxis.dotProduct(eye);
    data[14] = -zAxis.dotProduct(eye);

    data[15] = 1;
    return data
}
export function invert(data) {
    let d = (
        (data[0] * data[5] - data[4] * data[1]) * (data[10] * data[15] - data[14] * data[11]) -
        (data[0] * data[9] - data[8] * data[1]) * (data[6] * data[15] - data[14] * data[7]) +
        (data[0] * data[13] - data[12] * data[1]) * (data[6] * data[11] - data[10] * data[7]) +
        (data[4] * data[9] - data[8] * data[5]) * (data[2] * data[15] - data[14] * data[3]) -
        (data[4] * data[13] - data[12] * data[5]) * (data[2] * data[11] - data[10] * data[3]) +
        (data[8] * data[13] - data[12] * data[9]) * (data[2] * data[7] - data[6] * data[3])
    );
    let invertable = Math.abs(d) > 0.00000000001;

    if (invertable) {
        d = 1 / d;
        let m11 = data[0];
        let m21 = data[4];
        let m31 = data[8];
        let m41 = data[12];
        let m12 = data[1];
        let m22 = data[5];
        let m32 = data[9];
        let m42 = data[13];
        let m13 = data[2];
        let m23 = data[6];
        let m33 = data[10];
        let m43 = data[14];
        let m14 = data[3];
        let m24 = data[7];
        let m34 = data[11];
        let m44 = data[15];

        data[0] = d * (m22 * (m33 * m44 - m43 * m34) - m32 * (m23 * m44 - m43 * m24) + m42 * (m23 * m34 - m33 * m24));
        data[1] = -d * (m12 * (m33 * m44 - m43 * m34) - m32 * (m13 * m44 - m43 * m14) + m42 * (m13 * m34 - m33 * m14));
        data[2] = d * (m12 * (m23 * m44 - m43 * m24) - m22 * (m13 * m44 - m43 * m14) + m42 * (m13 * m24 - m23 * m14));
        data[3] = -d * (m12 * (m23 * m34 - m33 * m24) - m22 * (m13 * m34 - m33 * m14) + m32 * (m13 * m24 - m23 * m14));
        data[4] = -d * (m21 * (m33 * m44 - m43 * m34) - m31 * (m23 * m44 - m43 * m24) + m41 * (m23 * m34 - m33 * m24));
        data[5] = d * (m11 * (m33 * m44 - m43 * m34) - m31 * (m13 * m44 - m43 * m14) + m41 * (m13 * m34 - m33 * m14));
        data[6] = -d * (m11 * (m23 * m44 - m43 * m24) - m21 * (m13 * m44 - m43 * m14) + m41 * (m13 * m24 - m23 * m14));
        data[7] = d * (m11 * (m23 * m34 - m33 * m24) - m21 * (m13 * m34 - m33 * m14) + m31 * (m13 * m24 - m23 * m14));
        data[8] = d * (m21 * (m32 * m44 - m42 * m34) - m31 * (m22 * m44 - m42 * m24) + m41 * (m22 * m34 - m32 * m24));
        data[9] = -d * (m11 * (m32 * m44 - m42 * m34) - m31 * (m12 * m44 - m42 * m14) + m41 * (m12 * m34 - m32 * m14));
        data[10] = d * (m11 * (m22 * m44 - m42 * m24) - m21 * (m12 * m44 - m42 * m14) + m41 * (m12 * m24 - m22 * m14));
        data[11] = -d * (m11 * (m22 * m34 - m32 * m24) - m21 * (m12 * m34 - m32 * m14) + m31 * (m12 * m24 - m22 * m14));
        data[12] = -d * (m21 * (m32 * m43 - m42 * m33) - m31 * (m22 * m43 - m42 * m23) + m41 * (m22 * m33 - m32 * m23));
        data[13] = d * (m11 * (m32 * m43 - m42 * m33) - m31 * (m12 * m43 - m42 * m13) + m41 * (m12 * m33 - m32 * m13));
        data[14] = -d * (m11 * (m22 * m43 - m42 * m23) - m21 * (m12 * m43 - m42 * m13) + m41 * (m12 * m23 - m22 * m13));
        data[15] = d * (m11 * (m22 * m33 - m32 * m23) - m21 * (m12 * m33 - m32 * m13) + m31 * (m12 * m23 - m22 * m13));
    }
    return data;
}

export function decompose(data, orientationStyle = 'eulerAngles', target) {
    let q = Quaternion.CALCULATION_QUATERNION;
    let vec = target ? target : [new Vector3(), new Vector3(), new Vector3()];
    let mr = data;

    let pos = vec[0];
    pos.x = mr[12];
    pos.y = mr[13];
    pos.z = mr[14];
    mr[12] = 0;
    mr[13] = 0;
    mr[14] = 0;

    let scale = vec[2];

    scale.x = Math.sqrt(mr[0] * mr[0] + mr[1] * mr[1] + mr[2] * mr[2]);
    scale.y = Math.sqrt(mr[4] * mr[4] + mr[5] * mr[5] + mr[6] * mr[6]);
    scale.z = Math.sqrt(mr[8] * mr[8] + mr[9] * mr[9] + mr[10] * mr[10]);

    if (mr[0] * (mr[5] * mr[10] - mr[6] * mr[9])
        - mr[1] * (mr[4] * mr[10] - mr[6] * mr[8])
        + mr[2] * (mr[4] * mr[9] - mr[5] * mr[8]) < 0) {
        scale.z = -scale.z;
    }

    mr[0] /= scale.x;
    mr[1] /= scale.x;
    mr[2] /= scale.x;
    mr[4] /= scale.y;
    mr[5] /= scale.y;
    mr[6] /= scale.y;
    mr[8] /= scale.z;
    mr[9] /= scale.z;
    mr[10] /= scale.z;

    let rot = vec[1];
    let tr;
    switch (orientationStyle) {
        case Orientation3D.AXIS_ANGLE:
            rot.w = Math.acos((mr[0] + mr[5] + mr[10] - 1) / 2);

            let len = Math.sqrt((mr[6] - mr[9]) * (mr[6] - mr[9]) + (mr[8] - mr[2]) * (mr[8] - mr[2]) + (mr[1] - mr[4]) * (mr[1] - mr[4]));
            rot.x = (mr[6] - mr[9]) / len;
            rot.y = (mr[8] - mr[2]) / len;
            rot.z = (mr[1] - mr[4]) / len;

            break;
        case Orientation3D.QUATERNION:
            tr = mr[0] + mr[5] + mr[10];

            if (tr > 0) {
                rot.w = Math.sqrt(1 + tr) / 2;

                rot.x = (mr[6] - mr[9]) / (4 * rot.w);
                rot.y = (mr[8] - mr[2]) / (4 * rot.w);
                rot.z = (mr[1] - mr[4]) / (4 * rot.w);
            } else if (mr[0] > mr[5] && mr[0] > mr[10]) {
                rot.x = Math.sqrt(1 + mr[0] - mr[5] - mr[10]) / 2;

                rot.w = (mr[6] - mr[9]) / (4 * rot.x);
                rot.y = (mr[1] + mr[4]) / (4 * rot.x);
                rot.z = (mr[8] + mr[2]) / (4 * rot.x);
            } else if (mr[5] > mr[10]) {
                rot.y = Math.sqrt(1 + mr[5] - mr[0] - mr[10]) / 2;

                rot.x = (mr[1] + mr[4]) / (4 * rot.y);
                rot.w = (mr[8] - mr[2]) / (4 * rot.y);
                rot.z = (mr[6] + mr[9]) / (4 * rot.y);
            } else {
                rot.z = Math.sqrt(1 + mr[10] - mr[0] - mr[5]) / 2;

                rot.x = (mr[8] + mr[2]) / (4 * rot.z);
                rot.y = (mr[6] + mr[9]) / (4 * rot.z);
                rot.w = (mr[1] - mr[4]) / (4 * rot.z);
            }

            break;
        case Orientation3D.EULER_ANGLES:
            tr = mr[0] + mr[5] + mr[10];

            if (tr > 0) {
                q.w = Math.sqrt(1 + tr) / 2;

                q.x = (mr[6] - mr[9]) / (4 * q.w);
                q.y = (mr[8] - mr[2]) / (4 * q.w);
                q.z = (mr[1] - mr[4]) / (4 * q.w);
            } else if (mr[0] > mr[5] && mr[0] > mr[10]) {
                q.x = Math.sqrt(1 + mr[0] - mr[5] - mr[10]) / 2;

                q.w = (mr[6] - mr[9]) / (4 * q.x);
                q.y = (mr[1] + mr[4]) / (4 * q.x);
                q.z = (mr[8] + mr[2]) / (4 * q.x);
            } else if (mr[5] > mr[10]) {
                rot.y = Math.sqrt(1 + mr[5] - mr[0] - mr[10]) / 2;

                q.x = (mr[1] + mr[4]) / (4 * q.y);
                q.w = (mr[8] - mr[2]) / (4 * q.y);
                q.z = (mr[6] + mr[9]) / (4 * q.y);
            } else {
                q.z = Math.sqrt(1 + mr[10] - mr[0] - mr[5]) / 2;

                q.x = (mr[8] + mr[2]) / (4 * q.z);
                q.y = (mr[6] + mr[9]) / (4 * q.z);
                q.w = (mr[1] - mr[4]) / (4 * q.z);
            }
            q.getEulerAngles(rot);

            break;
    }

    vec[0] = pos;
    vec[1] = rot;
    vec[2] = scale;

    return vec;
}
export function makeMatrix44(r, p, s) {
    // Quaternion.CALCULATION_QUATERNION.fromEulerAngles(r.x, r.y, r.z);
    let DEGREES_TO_RADIANS = Math.PI / 180;
    let rawData = new Float32Array(16);

    let x = r.x * DEGREES_TO_RADIANS;
    let y = r.y * DEGREES_TO_RADIANS;
    let z = r.z * DEGREES_TO_RADIANS;
    let w = 0;

    let halfX = x * 0.5;
    let halfY = y * 0.5;
    let halfZ = z * 0.5;

    let cosX = Math.cos(halfX);
    let sinX = Math.sin(halfX);
    let cosY = Math.cos(halfY);
    let sinY = Math.sin(halfY);
    let cosZ = Math.cos(halfZ);
    let sinZ = Math.sin(halfZ);

    w = cosX * cosY * cosZ + sinX * sinY * sinZ;
    x = sinX * cosY * cosZ - cosX * sinY * sinZ;
    y = cosX * sinY * cosZ + sinX * cosY * sinZ;
    z = cosX * cosY * sinZ - sinX * sinY * cosZ;

    let x2 = x + x;
    let y2 = y + y;
    let z2 = z + z;

    let xx = x * x2;
    let xy = x * y2;
    let xz = x * z2;
    let yy = y * y2;
    let yz = y * z2;
    let zz = z * z2;
    let wx = w * x2;
    let wy = w * y2;
    let wz = w * z2;
    let sx = s.x;
    let sy = s.y;
    let sz = s.z;

    rawData[0] = (1 - (yy + zz)) * sx;
    rawData[1] = (xy + wz) * sx;
    rawData[2] = (xz - wy) * sx;
    rawData[3] = 0;
    rawData[4] = (xy - wz) * sy;
    rawData[5] = (1 - (xx + zz)) * sy;
    rawData[6] = (yz + wx) * sy;
    rawData[7] = 0;
    rawData[8] = (xz + wy) * sz;
    rawData[9] = (yz - wx) * sz;
    rawData[10] = (1 - (xx + yy)) * sz;
    rawData[11] = 0;
    rawData[12] = p.x;
    rawData[13] = p.y;
    rawData[14] = p.z;
    rawData[15] = 1;
    return rawData
}